#include "driver_device.hpp"

#include "detail/motec_driver.hpp"

#include <log/log.hpp>

namespace sros::device
{
    DriverDevice::DriverDevice(std::shared_ptr<McuDevice> mcu_device)
        : device_impl<cfg::DriversCfg>(), mcu_(mcu_device), h_driver_(nullptr), v_driver_(nullptr), p_driver_(nullptr), r_driver_(nullptr)
    {
    }

    DriverDevice::~DriverDevice()
    {
    }

    RetBool DriverDevice::init()
    {
        // 加载配置
        auto ret = load_config();
        if (!ret.v)
        {
            return ret;
        }

        // 根据配置创建电机驱动
        ret = init_drivers_();
        if (!ret.v)
        {
            return ret;
        }

        // 初始化电机驱动
        if (h_driver_)
        {
            h_driver_->init();
        }

        if (v_driver_)
        {
            v_driver_->init();
        }

        if (p_driver_)
        {
            p_driver_->init();
        }

        if (r_driver_)
        {
            r_driver_->init();
        }

        return {true};
    }

    void DriverDevice::install_event(EventCb cb)
    {
        if (h_driver_)
        {
            h_driver_->install_event([this, cb](const uint8_t &ev_id, const uint32_t &data, const std::string &msg)
            {
                cb(ev_id, 0, data, msg);
            });
        }

        if (v_driver_)
        {
            v_driver_->install_event([this, cb](const uint8_t &ev_id, const uint32_t &data, const std::string &msg)
            {
                cb(ev_id, 1, data, msg);
            });
        }

        if (p_driver_)
        {
            p_driver_->install_event([this, cb](const uint8_t &ev_id, const uint32_t &data, const std::string &msg)
            {
                cb(ev_id, 2, data, msg);
            });
        }

        if (r_driver_)
        {
            r_driver_->install_event([this, cb](const uint8_t &ev_id, const uint32_t &data, const std::string &msg)
            {
                cb(ev_id, 3, data, msg);
            });
        }
    }

    void DriverDevice::run()
    {
        if (h_driver_)
        {
            h_driver_->run();
        }

        if (v_driver_)
        {
            v_driver_->run();
        }

        if (p_driver_)
        {
            p_driver_->run();
        }

        if (r_driver_)
        {
            r_driver_->run();
        }
    }

    void DriverDevice::close()
    {
        {
            std::lock_guard<std::mutex> lck(mtx_);

            if (h_driver_)
            {
                h_driver_->close();
                h_driver_ = nullptr;
            }

            if (v_driver_)
            {
                v_driver_->close();
                v_driver_ = nullptr;
            }

            if (p_driver_)
            {
                p_driver_->close();
                p_driver_ = nullptr;
            }

            if (r_driver_)
            {
                r_driver_->close();
                r_driver_ = nullptr;
            }
        }

        if (config_)
        {
            config_->deinit();
            config_ = nullptr;
        }
    }

    void DriverDevice::parse(const device::proto::RspCanMsg &can_msg)
    {
        std::lock_guard<std::mutex> lck(mtx_);

        if (h_driver_)
        {
            if (h_driver_->param().can_index == can_msg.index)
            {
                h_driver_->parse(can_msg);
            }
        }

        if (v_driver_)
        {
            if (v_driver_->param().can_index == can_msg.index)
            {
                v_driver_->parse(can_msg);
            }
        }

        if (p_driver_)
        {
            if (p_driver_->param().can_index == can_msg.index)
            {
                p_driver_->parse(can_msg);
            }
        }

        if (r_driver_)
        {
            if (r_driver_->param().can_index == can_msg.index)
            {
                r_driver_->parse(can_msg);
            }
        }
    }

    RetBool DriverDevice::set_spd(const uint8_t &dir, const int32_t &spd)
    {
        switch (static_cast<cfg::DriverDir>(dir))
        {
        case cfg::DriverDir::H:
        {
            if (h_driver_)
            {
                auto ret = h_driver_->set_spd(spd);
                return ret;
            }
            return {false, "h_driver_ is nullptr"};
        }
        break;
        case cfg::DriverDir::V:
        {
            if (v_driver_)
            {
                auto ret = v_driver_->set_spd(spd);
                return ret;
            }
            return {false, "v_driver_ is nullptr"};
        }
        break;
        case cfg::DriverDir::P:
        {
            if (p_driver_)
            {
                auto ret = p_driver_->set_spd(spd);
                return ret;
            }
            return {false, "p_driver_ is nullptr"};
        }
        break;
        case cfg::DriverDir::R:
        {
            if (r_driver_)
            {
                auto ret = r_driver_->set_spd(spd);
                return ret;
            }
            return {false, "r_driver_ is nullptr"};
        }
        break;
        default:
        {
            return {false, "not support DriverDir"};
        }
        break;
        }
    }

    RetBool DriverDevice::set_current(const cfg::DriverDir &dir, const int32_t &current)
    {
        switch (dir)
        {
        case cfg::DriverDir::H:
        {
            if (h_driver_)
            {
                auto ret = h_driver_->set_current(current);
                return ret;
            }
            return {false, "h_driver_ is nullptr"};
        }
        break;
        case cfg::DriverDir::V:
        {
            if (v_driver_)
            {
                auto ret = v_driver_->set_current(current);
                return ret;
            }
            return {false, "v_driver_ is nullptr"};
        }
        break;
        case cfg::DriverDir::P:
        {
            if (p_driver_)
            {
                auto ret = p_driver_->set_current(current);
                return ret;
            }
            return {false, "p_driver_ is nullptr"};
        }
        break;
        case cfg::DriverDir::R:
        {
            if (r_driver_)
            {
                auto ret = r_driver_->set_current(current);
                return ret;
            }
            return {false, "r_driver_ is nullptr"};
        }
        break;
        default:
        {
            return {false, "not support DriverDir"};
        }
        break;
        }
    }

    RetBool DriverDevice::set_control(const uint16_t &control_word)
    {
        if (h_driver_)
        {
            auto driver = std::dynamic_pointer_cast<detail::MotecDriver>(h_driver_);
            auto ret = driver->set_control(control_word);
            return ret;
        }
        return {false, "h_driver_ is nullptr"};
    }

    RetBool DriverDevice::data_up(const cfg::DriverDir &dir, detail::driver_impl::DataUp &data)
    {
        switch (dir)
        {
        case cfg::DriverDir::H:
        {
            if (h_driver_)
            {
                data = h_driver_->data_up();
                return {true};
            }
            return {false, "h_driver_ is nullptr"};
        }
        break;
        case cfg::DriverDir::V:
        {
            if (v_driver_)
            {
                data = v_driver_->data_up();
                return {true};
            }
            return {false, "v_driver_ is nullptr"};
        }
        break;
        case cfg::DriverDir::P:
        {
            if (p_driver_)
            {
                data = p_driver_->data_up();
                return {true};
            }
            return {false, "p_driver_ is nullptr"};
        }
        break;
        case cfg::DriverDir::R:
        {
            if (r_driver_)
            {
                data = r_driver_->data_up();
                return {true};
            }
            return {false, "r_driver_ is nullptr"};
        }
        break;
        default:
        {
            return {false, "not support DriverDir"};
        }
        break;
        }
    }

    RetBool DriverDevice::data_down(const cfg::DriverDir &dir, detail::driver_impl::DataDown &data)
    {
        switch (dir)
        {
        case cfg::DriverDir::H:
        {
            if (h_driver_)
            {
                data = h_driver_->data_down();
                return {true};
            }
            return {false, "h_driver_ is nullptr"};
        }
        break;
        case cfg::DriverDir::V:
        {
            if (v_driver_)
            {
                data = v_driver_->data_down();
                return {true};
            }
            return {false, "v_driver_ is nullptr"};
        }
        break;
        case cfg::DriverDir::P:
        {
            if (p_driver_)
            {
                data = p_driver_->data_down();
                return {true};
            }
            return {false, "p_driver_ is nullptr"};
        }
        break;
        case cfg::DriverDir::R:
        {
            if (r_driver_)
            {
                data = r_driver_->data_down();
                return {true};
            }
            return {false, "r_driver_ is nullptr"};
        }
        break;
        default:
        {
            return {false, "not support DriverDir"};
        }
        break;
        }
    }

    RetBool DriverDevice::info(const cfg::DriverDir &dir, detail::Info &info)
    {
        switch (dir)
        {
        case cfg::DriverDir::H:
        {
            if (h_driver_)
            {
                info = h_driver_->info();
                return {true};
            }
            return {false, "h_driver_ is nullptr"};
        }
        break;
        case cfg::DriverDir::V:
        {
            if (v_driver_)
            {
                info = v_driver_->info();
                return {true};
            }
            return {false, "v_driver_ is nullptr"};
        }
        break;
        case cfg::DriverDir::P:
        {
            if (p_driver_)
            {
                info = p_driver_->info();
                return {true};
            }
            return {false, "p_driver_ is nullptr"};
        }
        break;
        case cfg::DriverDir::R:
        {
            if (r_driver_)
            {
                info = r_driver_->info();
                return {true};
            }
            return {false, "r_driver_ is nullptr"};
        }
        break;
        default:
        {
            return {false, "not support DriverDir"};
        }
        break;
        }
    }

    RetBool DriverDevice::param(const cfg::DriverDir &dir, detail::driver_impl::Param &param)
    {
        switch (dir)
        {
        case cfg::DriverDir::H:
        {
            if (h_driver_)
            {
                param = h_driver_->param();
                return {true};
            }
            return {false, "h_driver_ is nullptr"};
        }
        break;
        case cfg::DriverDir::V:
        {
            if (v_driver_)
            {
                param = v_driver_->param();
                return {true};
            }
            return {false, "v_driver_ is nullptr"};
        }
        break;
        case cfg::DriverDir::P:
        {
            if (p_driver_)
            {
                param = p_driver_->param();
                return {true};
            }
            return {false, "p_driver_ is nullptr"};
        }
        break;
        case cfg::DriverDir::R:
        {
            if (r_driver_)
            {
                param = r_driver_->param();
                return {true};
            }
            return {false, "r_driver_ is nullptr"};
        }
        break;
        default:
        {
            return {false, "not support DriverDir"};
        }
        break;
        }
    }

    RetBool DriverDevice::load_config()
    {
        if (config_)
        {
            return {true};
        }

        std::string root = util::env_var("HOME");
        if (root.empty())
        {
            root = ".";
        }
        std::string config_path = util::format("{}/.sros/{}/{}/config/driver_config.json", root, MODULE, NODE);

        config_ = std::make_shared<commons::Config<cfg::DriversCfg>>();
        auto ret = config_->init(config_path);
        if (!ret.v)
        {
            return ret;
        }

        return config_->load();
    }

    RetBool DriverDevice::init_drivers_()
    {
        if (config_ == nullptr)
        {
            return {false, "config is nullptr"};
        }

        cfg::DriversCfg cfg_;
        auto ret = config_->get(cfg_);
        if (!ret.v)
        {
            return ret;
        }

        // 根据驱动类型 创建实例
        auto create_driver = [this](const cfg::DriverCfg &driver_cfg) -> std::shared_ptr<detail::driver_impl>
        {
            switch (driver_cfg.type)
            {
            case cfg::DriverType::Motec:
            {
                detail::MotecDriver::Param param{driver_cfg.id, driver_cfg.motor_dir, driver_cfg.can_index,
                                                 driver_cfg.resolution, driver_cfg.diameter, driver_cfg.gear_ratio};
                return std::make_shared<detail::MotecDriver>(param, mcu_);
            }
            break;
            default:
            {
                return nullptr;
            }
            break;
            }
        };

        uint8_t mode = 0; // 0: unkown 1: 单电机模式 2: 双电机模式

        // 判断电机驱动模式
        if (cfg_.H.size() == 1 && cfg_.H.at(0).enable) // 单电机驱动模式
        {
            mode = 1;
        }
        else if (cfg_.H.size() == 2 && cfg_.H.at(0).enable && cfg_.H.at(1).enable) // 双电机驱动模式
        {
            mode = 2;
        }
        else
        {
            mode = 0;
        }

        if (mode == 0) // 不支持的模式配置
        {
            LOG_WARN("unsupport H config");
        }

        if (mode == 1) // 单电机驱动模式
        {
            h_driver_ = create_driver(cfg_.H.at(0));
        }

        if (mode == 2)
        {
            h_driver_ = nullptr;
        }

        return {true};
    }
}